// https://github.com/32bitmicro/newlib-nano-1.0/blob/master/newlib/libc/machine/xstormy16/tiny-malloc.c

/* A replacement malloc with:
   - Much reduced code size;
   - Smaller RAM footprint;
   - The ability to handle downward-growing heaps;
   but
   - Slower;
   - Probably higher memory fragmentation;
   - Doesn't support threads (but, if it did support threads,
     it wouldn't need a global lock, only a compare-and-swap instruction);
   - Assumes the maximum alignment required is the alignment of a pointer;
   - Assumes that memory is already there and doesn't need to be allocated.

* Synopsis of public routines

  malloc(size_t n);
     Return a pointer to a newly allocated chunk of at least n bytes, or null
     if no space is available.
  free(void* p);
     Release the chunk of memory pointed to by p, or no effect if p is null.
  realloc(void* p, size_t n);
     Return a pointer to a chunk of size n that contains the same data
     as does chunk p up to the minimum of (n, p's size) bytes, or null
     if no space is available. The returned pointer may or may not be
     the same as p. If p is null, equivalent to malloc.  Unless the
     #define REALLOC_ZERO_BYTES_FREES below is set, realloc with a
     size argument of zero (re)allocates a minimum-sized chunk.
  memalign(size_t alignment, size_t n);
     Return a pointer to a newly allocated chunk of n bytes, aligned
     in accord with the alignment argument, which must be a power of
     two.  Will fail if 'alignment' is too large.
  calloc(size_t unit, size_t quantity);
     Returns a pointer to quantity * unit bytes, with all locations
     set to zero.
  cfree(void* p);
     Equivalent to free(p).
  malloc_trim(size_t pad);
     Release all but pad bytes of freed top-most memory back
     to the system. Return 1 if successful, else 0.
  malloc_usable_size(void* p);
     Report the number usable allocated bytes associated with allocated
     chunk p. This may or may not report more bytes than were requested,
     due to alignment and minimum size constraints.
  malloc_stats();
     Prints brief summary statistics on stderr.
  mallinfo()
     Returns (by copy) a struct containing various summary statistics.
  mallopt(int parameter_number, int parameter_value)
     Changes one of the tunable parameters described below. Returns
     1 if successful in changing the parameter, else 0.  Actually, returns 0
     always, as no parameter can be changed.
*/

#ifndef MALLOC_DIRECTION
#define MALLOC_DIRECTION 1
#endif

#include <stddef.h>
#include "string.h"

void* malloc(size_t);
void
free(void*);
void*
realloc(void*, size_t);
void* memalign(size_t, size_t);
void* valloc(size_t);
void* pvalloc(size_t);
void* calloc(size_t, size_t);
void
cfree(void*);
int malloc_trim(size_t);
size_t
malloc_usable_size(void*);
void
malloc_stats(void);
int
mallopt(int, int);
struct mallinfo
mallinfo(void);

typedef struct freelist_entry {
  size_t size;
  struct freelist_entry* next;
} * fle;

static fle __malloc_freelist = 0;
static void* __malloc_end    = 0;
// fle __malloc_freelist;

/* Return the number of bytes that need to be added to X to make it
   aligned to an ALIGN boundary.  ALIGN must be a power of 2.  */
#define M_ALIGN(x, align) (-(size_t)(x) & ((align)-1))

/* Return the number of bytes that need to be subtracted from X to make it
   aligned to an ALIGN boundary.  ALIGN must be a power of 2.  */
#define M_ALIGN_SUB(x, align) ((size_t)(x) & ((align)-1))

extern char* __malloc_start;
// extern char *__malloc_zone_stop; /* dkohlbre: added since our malloc region
// is static */

/* This is the minimum gap allowed between __malloc_end and the top of
   the stack.  This is only checked for when __malloc_end is
   decreased; if instead the stack grows into the heap, silent data
   corruption will result.  */
#define MALLOC_MINIMUM_GAP 32

#ifdef __xstormy16__
register void* stack_pointer asm("r15");
#define MALLOC_LIMIT stack_pointer
#else
#define MALLOC_LIMIT                                                         \
  __builtin_frame_address(0) /* dkohlbre: modification for our malloc region \
                              */
#endif

#if MALLOC_DIRECTION < 0
#define CAN_ALLOC_P(required)                                            \
  (((size_t)__malloc_end - (size_t)MALLOC_LIMIT - MALLOC_MINIMUM_GAP) >= \
   (required))
#else
#define CAN_ALLOC_P(required)                                            \
  (((size_t)MALLOC_LIMIT - (size_t)__malloc_end - MALLOC_MINIMUM_GAP) >= \
   (required))
#endif

/* real_size is the size we actually have to allocate, allowing for
   overhead and alignment.  */
#define REAL_SIZE(sz)                                    \
  ((sz) < sizeof(struct freelist_entry) - sizeof(size_t) \
       ? sizeof(struct freelist_entry)                   \
       : sz + sizeof(size_t) + M_ALIGN(sz, sizeof(size_t)))

void*
malloc(size_t sz) {
  fle* nextfree;
  fle block;

  /* dkohlbre: Force init of malloc_end, it wasn't always gettng set */
  if (__malloc_end == NULL) {
    __malloc_end = &__malloc_start;
  }

  /* real_size is the size we actually have to allocate, allowing for
     overhead and alignment.  */
  size_t real_size = REAL_SIZE(sz);

  /* Look for the first block on the freelist that is large enough.  */
  for (nextfree = &__malloc_freelist; *nextfree;
       nextfree = &(*nextfree)->next) {
    block = *nextfree;

    if (block->size >= real_size) {
      /* If the block found is just the right size, remove it from
         the free list.  Otherwise, split it.  */
      if (block->size < real_size + sizeof(struct freelist_entry)) {
        *nextfree = block->next;
        return (void*)&block->next;
      } else {
        size_t newsize    = block->size - real_size;
        fle newnext       = block->next;
        *nextfree         = (fle)((size_t)block + real_size);
        (*nextfree)->size = newsize;
        (*nextfree)->next = newnext;
        goto done;
      }
    }

    /* If this is the last block on the freelist, and it was too small,
 enlarge it.  */
    if (!block->next && __malloc_end == (void*)((size_t)block + block->size)) {
      size_t moresize = real_size - block->size;
      if (!CAN_ALLOC_P(moresize)) return NULL;

      *nextfree = NULL;
      if (MALLOC_DIRECTION < 0) {
        block = __malloc_end = (void*)((size_t)block - moresize);
      } else {
        __malloc_end = (void*)((size_t)block + real_size);
      }

      goto done;
    }
  }

  /* No free space at the end of the free list.  Allocate new space
     and use that.  */

  if (!CAN_ALLOC_P(real_size)) return NULL;

  if (MALLOC_DIRECTION > 0) {
    block        = __malloc_end;
    __malloc_end = (void*)((size_t)__malloc_end + real_size);
  } else {
    block = __malloc_end = (void*)((size_t)__malloc_end - real_size);
  }
done:
  block->size = real_size;
  return (void*)&block->next;
}

void
free(void* block_p) {
  fle* nextfree;
  fle block = (fle)((size_t)block_p - offsetof(struct freelist_entry, next));

  if (block_p == NULL) return;

  /* Look on the freelist to see if there's a free block just before
     or just after this block.  */
  for (nextfree = &__malloc_freelist; *nextfree;
       nextfree = &(*nextfree)->next) {
    fle thisblock = *nextfree;
    if ((size_t)thisblock + thisblock->size == (size_t)block) {
      thisblock->size += block->size;
      if (MALLOC_DIRECTION > 0 && thisblock->next &&
          (size_t)block + block->size == (size_t)thisblock->next) {
        thisblock->size += thisblock->next->size;
        thisblock->next = thisblock->next->next;
      }
      return;
    } else if ((size_t)thisblock == (size_t)block + block->size) {
      if (MALLOC_DIRECTION < 0 && thisblock->next &&
          (size_t)block == ((size_t)thisblock->next + thisblock->next->size)) {
        *nextfree = thisblock->next;
        thisblock->next->size += block->size + thisblock->size;
      } else {
        block->size += thisblock->size;
        block->next = thisblock->next;
        *nextfree   = block;
      }
      return;
    } else if (
        (MALLOC_DIRECTION > 0 && (size_t)thisblock > (size_t)block) ||
        (MALLOC_DIRECTION < 0 && (size_t)thisblock < (size_t)block))
      break;
  }

  block->next = *nextfree;
  *nextfree   = block;
  return;
}

void*
realloc(void* block_p, size_t sz) {
  fle block = (fle)((size_t)block_p - offsetof(struct freelist_entry, next));
  size_t real_size = REAL_SIZE(sz);
  size_t old_real_size;

  if (block_p == NULL) return malloc(sz);

  old_real_size = block->size;

  /* Perhaps we need to allocate more space.  */
  if (old_real_size < real_size) {
    void* result;
    size_t old_size = old_real_size - sizeof(size_t);

    /* Need to allocate, copy, and free.  */
    result = malloc(sz);
    if (result == NULL) return NULL;
    memcpy(result, block_p, old_size < sz ? old_size : sz);
    free(block_p);
    return result;
  }
  /* Perhaps we can free some space.  */
  if (old_real_size - real_size >= sizeof(struct freelist_entry)) {
    fle newblock   = (fle)((size_t)block + real_size);
    block->size    = real_size;
    newblock->size = old_real_size - real_size;
    free(&newblock->next);
  }
  return block_p;
}

void*
calloc(size_t n, size_t elem_size) {
  void* result;
  size_t sz = n * elem_size;
  result    = malloc(sz);
  if (result != NULL) memset(result, 0, sz);
  return result;
}

void
cfree(void* p) {
  free(p);
}

void*
memalign(size_t align, size_t sz) {
  fle* nextfree;
  fle block;

  /* real_size is the size we actually have to allocate, allowing for
     overhead and alignment.  */
  size_t real_size = REAL_SIZE(sz);

  /* Some sanity checking on 'align'. */
  if ((align & (align - 1)) != 0 || align <= 0) return NULL;

  /* Look for the first block on the freelist that is large enough.  */
  /* One tricky part is this: We want the result to be a valid pointer
     to free.  That means that there has to be room for a size_t
     before the block.  If there's additional space before the block,
     it should go on the freelist, or it'll be lost---we could add it
     to the size of the block before it in memory, but finding the
     previous block is expensive.  */
  for (nextfree = &__malloc_freelist;; nextfree = &(*nextfree)->next) {
    size_t before_size;
    size_t old_size;

    /* If we've run out of free blocks, allocate more space.  */
    if (!*nextfree) {
      old_size = real_size;
      if (MALLOC_DIRECTION < 0) {
        old_size += M_ALIGN_SUB(
            ((size_t)__malloc_end - old_size + sizeof(size_t)), align);
        if (!CAN_ALLOC_P(old_size)) return NULL;
        block = __malloc_end = (void*)((size_t)__malloc_end - old_size);
      } else {
        block = __malloc_end;
        old_size += M_ALIGN((size_t)__malloc_end + sizeof(size_t), align);
        if (!CAN_ALLOC_P(old_size)) return NULL;
        __malloc_end = (void*)((size_t)__malloc_end + old_size);
      }
      *nextfree   = block;
      block->size = old_size;
      block->next = NULL;
    } else {
      block    = *nextfree;
      old_size = block->size;
    }

    before_size = M_ALIGN(&block->next, align);
    if (before_size != 0)
      before_size = sizeof(*block) + M_ALIGN(&(block + 1)->next, align);

    /* If this is the last block on the freelist, and it is too small,
 enlarge it.  */
    if (!block->next && old_size < real_size + before_size &&
        __malloc_end == (void*)((size_t)block + block->size)) {
      if (MALLOC_DIRECTION < 0) {
        size_t moresize = real_size - block->size;
        moresize += M_ALIGN_SUB((size_t)&block->next - moresize, align);
        if (!CAN_ALLOC_P(moresize)) return NULL;
        block = __malloc_end = (void*)((size_t)block - moresize);
        block->next          = NULL;
        block->size = old_size = old_size + moresize;
        before_size            = 0;
      } else {
        if (!CAN_ALLOC_P(before_size + real_size - block->size)) return NULL;
        __malloc_end = (void*)((size_t)block + before_size + real_size);
        block->size = old_size = before_size + real_size;
      }

      /* Two out of the four cases below will now be possible; which
         two depends on MALLOC_DIRECTION.  */
    }

    if (old_size >= real_size + before_size) {
      /* This block will do.  If there needs to be space before it,
         split the block.  */
      if (before_size != 0) {
        fle old_block = block;

        old_block->size = before_size;
        block           = (fle)((size_t)block + before_size);

        /* If there's no space after the block, we're now nearly
                 done; just make a note of the size required.
           Otherwise, we need to create a new free space block.  */
        if (old_size - before_size <=
            real_size + sizeof(struct freelist_entry)) {
          block->size = old_size - before_size;
          return (void*)&block->next;
        } else {
          fle new_block;
          new_block       = (fle)((size_t)block + real_size);
          new_block->size = old_size - before_size - real_size;
          if (MALLOC_DIRECTION > 0) {
            new_block->next = old_block->next;
            old_block->next = new_block;
          } else {
            new_block->next = old_block;
            *nextfree       = new_block;
          }
          goto done;
        }
      } else {
        /* If the block found is just the right size, remove it from
     the free list.  Otherwise, split it.  */
        if (old_size <= real_size + sizeof(struct freelist_entry)) {
          *nextfree = block->next;
          return (void*)&block->next;
        } else {
          size_t newsize    = old_size - real_size;
          fle newnext       = block->next;
          *nextfree         = (fle)((size_t)block + real_size);
          (*nextfree)->size = newsize;
          (*nextfree)->next = newnext;
          goto done;
        }
      }
    }
  }

done:
  block->size = real_size;
  return (void*)&block->next;
}

void*
valloc(size_t sz) {
  return memalign(128, sz);
}
void*
pvalloc(size_t sz) {
  return memalign(128, sz + M_ALIGN(sz, 128));
}

#ifdef DEFINE_MALLINFO
#include "malloc.h"

struct mallinfo
mallinfo(void) {
  struct mallinfo r;
  fle fr;
  size_t free_size;
  size_t total_size;
  size_t free_blocks;

  memset(&r, 0, sizeof(r));

  free_size   = 0;
  free_blocks = 0;
  for (fr = __malloc_freelist; fr; fr = fr->next) {
    free_size += fr->size;
    free_blocks++;
    if (!fr->next) {
      int atend;
      if (MALLOC_DIRECTION > 0)
        atend = (void*)((size_t)fr + fr->size) == __malloc_end;
      else
        atend = (void*)fr == __malloc_end;
      if (atend) r.keepcost = fr->size;
    }
  }

  if (MALLOC_DIRECTION > 0)
    total_size = (char*)__malloc_end - (char*)&__malloc_start;
  else
    total_size = (char*)&__malloc_start - (char*)__malloc_end;

  r.arena    = total_size;
  r.fordblks = free_size;
  r.uordblks = total_size - free_size;
  r.ordblks  = free_blocks;
  return r;
}
#endif

#ifdef DEFINE_MALLOC_STATS
#include <stdio.h>
#include "malloc.h"

void
malloc_stats(void) {
  struct mallinfo i;
  FILE* fp;

  fp = stderr;
  i  = mallinfo();
  fprintf(
      fp, "malloc has reserved %u bytes between %p and %p\n", i.arena,
      &__malloc_start, __malloc_end);
  fprintf(fp, "there are %u bytes free in %u chunks\n", i.fordblks, i.ordblks);
  fprintf(
      fp, "of which %u bytes are at the end of the reserved space\n",
      i.keepcost);
  fprintf(fp, "and %u bytes are in use.\n", i.uordblks);
}
#endif

#ifdef DEFINE_MALLOC_USABLE_SIZE
size_t
malloc_usable_size(void* block_p) {
  fle block = (fle)((size_t)block_p - offsetof(struct freelist_entry, next));
  return block->size - sizeof(size_t);
}
#endif

#ifdef DEFINE_MALLOPT
int
mallopt(int n, int v) {
  (void)n;
  (void)v;
  return 0;
}
#endif
